﻿/*                       NOTICE
 * 
 * This software was produced for the U. S. Government
 * under Contract No. FA8721-10-C-0001, and is
 * subject to the Rights in Noncommercial Computer Software
 * and Noncommercial Computer Software Documentation Clause
 * (DFARS) 252.227-7014 (JUN 1995)
 * 
 * Approved for Public Release: 11-2435
 * © 2011 The MITRE Corporation. All Rights Reserved. 
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 * http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */



using System;
using System.Threading;
using System.Windows;
using System.Runtime.InteropServices;
using xn;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework;
using GRIPXNAKinect;
using System.Collections.Generic;
using MXSE.Renderers.TileManager;
using System.Drawing;
using System.Drawing.Imaging;
using System.ComponentModel;
using System.Windows.Forms;
using System.Collections;

namespace Nui
{
    /// <summary>
    /// Represents a Natural User Interface (image and depth) sensor handler.
    /// </summary>
    public class NuiSensor : INUI
    {
        #region Constants

        /// <summary>
        /// Default configuration file path.
        /// </summary>
        private const string CONFIGURATION = @"SamplesConfig.xml";

        /// <summary>
        /// Horizontal bitmap dpi.
        /// </summary>
        private readonly int DPI_X = 96;

        /// <summary>
        /// Vertical bitmap dpi.
        /// </summary>
        private readonly int DPI_Y = 96;

        #endregion

        #region Members

        /// <summary>
        /// Thread responsible for image and depth camera updates.
        /// </summary>
        private Thread _cameraThread;

        /// <summary>
        /// Indicates whether the thread is running.
        /// </summary>
        private bool _isRunning = true;
        private bool _alreadySetCentroids = false;

        private int xMin, xMax, yMin, yMax, zMin, zMax;
        private int xMid, yMid;

        public int minX1;
        public int maxX1;
        public int minX2;
        public int maxX2;
        int x, y;

        private long lostTime1 = 0;
        private long lostTime2 = 0;
        public double delta1 = 0;
        public double delta2 = 0;
        public int minZ1 = 1000;
        public int minZ2 = 1000;
        public double threshold1;
        public double threshold2;

        private Vector3 centroid1;
        private Vector3 centroid2;
        private Vector3 lastcentroid1;
        private Vector3 lastcentroid2;
        private List<Vector3> cluster1;
        private List<Vector3> cluster2;
        List<Vector3> clusterA = new List<Vector3>();
        List<Vector3> clusterB = new List<Vector3>();
        public bool c1Open = true;
        public bool c2Open = true;

        // MANUAL OVERRIDE! SWITCH THIS FOR DEPTH IMAGE VS RGB
        public bool depthMode = false;
        public bool highlightCapturedPoints = true;

        public bool lockedMode1;
        public bool lockedMode2;
        public long lockTime1;
        public long lockTime2;
        public ushort[] previousDepthImage;

        public int[] fingers1 = new int[5];
        public int[] fingers2 = new int[5];

        /// <summary>
        /// Raw image metadata.
        /// </summary>
        private ImageMetaData _imgMD = new ImageMetaData();

        /// <summary>
        /// Depth image metadata.
        /// </summary>
        private DepthMetaData _depthMD = new DepthMetaData();

        private Texture2D depthImageTexture;


        #endregion

        #region Properties

        #region Bitmap properties
        private Bitmap depthBitmap;
        private List<Vector3> depthThreshholdBreakingPoints = new List<Vector3>();

        private int sortFunc(PointCV a, PointCV b) {
            return (int)(a.y - b.y);
        }

        public void GenerateFingersCV(ref int[] fingers, List<Vector3> cluster, Vector3 centroid, int minx, int maxx) {
            unsafe {
                lock (this) {
                    // Finding a convex hull in the plane
                    // This program requires .Net version 2.0.
                    // Peter Sestoft (sestoft@dina.kvl.dk) * Java 2000-10-07, GC# 2001-10-27
                    int i;
                    // Find convex hull
                    PointCV[] pts = new PointCV[cluster.Count];
                    for (i = 0; i < cluster.Count; i++)
                        pts[i] = new PointCV(cluster[i].X, cluster[i].Y);
                    PointCV[] chpts = Convexhull.convexhull(pts);

                    // Calculate linear regression
                    /*double sumx = 0, sumy = 0, sumxy = 0, sumxx = 0;
                    for (i = 0; i < cluster.Count; i++) {
                        sumx += cluster[i].X;
                        sumy += cluster[i].Y;
                        sumxy += (cluster[i].X * cluster[i].Y);
                        sumxx += (cluster[i].X * cluster[i].X);
                    }
                    double slope = ((cluster.Count * sumxy) - (sumx * sumy)) / ((cluster.Count * sumxx) - (sumx * sumx));

                    // Use linear regression slope to find angle hand is oriented at
                    double normalized = slope/Math.Sqrt(1+slope * slope);
                    double angle = Math.Acos(normalized) * 180.0 / Math.PI;
                    Console.WriteLine(slope + " " + normalized + " " + angle);

                    // Transform hand points so they appear upright
                    PointCV[] trans = new PointCV[chpts.Length];
                    for (i = 0; i < chpts.Length; i++) {
                        trans[i] = new PointCV(chpts[i].x * Math.Cos(angle) - chpts[i].y * Math.Sin(angle), chpts[i].x * Math.Sin(angle) + chpts[i].y * Math.Cos(angle));
                    }*/

                    // Sort points by Y
                    Array.Sort(chpts, sortFunc);

                    PointCV[] tips = new PointCV[5];
                    for (i = 0; i < 5; i++) {
                        tips[i] = null;
                    }
                    int cnt = 0;
                    i = 0;
                    // Find fingertips
                    while (cnt < 5 && i < chpts.Length) {
                        if (cnt == 0) {
                            tips[cnt] = chpts[i];
                            cnt++;
                        } else {
                            bool flag = true;
                            for (int j = 0; j < cnt; j++) {
                                // Check to make sure points aren't right next to each other (same finger)
                                int diff = (int)Math.Abs(chpts[i].x - tips[j].x);
                                if (diff < ((1 + maxx - minx) - 10) / 5) {
                                    flag = false;
                                    break;
                                }
                            }
                            if (flag) {
                                double distance = Math.Sqrt(Math.Pow(chpts[i].x - centroid.X, 2)+Math.Pow(chpts[i].y - centroid.Y, 2));
                                // First-pass check to ignore closed fingers and ignore non-fingers
                                if (distance > 50 && centroid.Y - chpts[i].y > 0) {
                                    tips[cnt] = chpts[i];
                                    cnt++;
                                }
                            }
                        }
                        i++;
                    }

                    // Transfer found fingers to output array
                    for (i = 0; i < 5; i++) {
                        if (tips[i] == null) {
                            fingers[i] = 0;
                            continue;
                        }
                        int r = (int)tips[i].y;
                        int c = (int)tips[i].x;
                        fingers[i] = r * depthBitmap.Width + c;
                    }
                }
            }
        }

        #region Unused Finger Detection Algorithm
        /*public void GenerateFingersKC(ref int[] fingers, ref List<Vector3> cluster, int minx, int maxx, int miny, int maxy) {
            unsafe {
                lock (this) {
                    int width = 1+maxx - minx;
                    int height = 1+maxy - miny;
                    bool prevPixelBlack = true, prevPixelHand = false, prevPixelBorder = false;
                    int bufferSize = width*height;
                    uint[] depth_mid = new uint[bufferSize*3];
                    int[] borderPixels = new int[bufferSize];
                    int borderCnt = 0;
                    int lastHandIndex = 0;
                    int[] segBuff = new int[bufferSize];

                    for (int i = 0; i < 5; i++) {
                        fingers[i] = 0;
                    }

                    for (int i = 0; i < bufferSize; i++) {
                        depth_mid[i*3] = 0;
                        depth_mid[i*3+1] = 0;
                        depth_mid[i*3+2] = 0;
                        borderPixels[i] = 0;
                        segBuff[i] = 0;
                    }

                    Vector3 lastpoint = new Vector3();
                    for (int i = 0; i < cluster.Count; i++) {
                        Vector3 point = cluster[i];
                        int r = (int)point.Y - miny;
                        int c = (int)point.X - minx;

                        if (i != 0 && point.X != lastpoint.X + 1) {
                            int lastc = (int)lastpoint.X - minx;
                            int lastr = (int)lastpoint.Y - miny;
                            depth_mid[3*(lastr * width + lastc)] = 255;
                            depth_mid[3*(lastr * width + lastc)+1] = 255;
                            depth_mid[3*(lastr * width + lastc)+2] = 255;
                            borderPixels[++borderCnt] = lastr * width + lastc;
                            prevPixelBlack = true;
                            prevPixelHand = false;
                            prevPixelBorder = false;
                        }
                        if (prevPixelBlack || i == cluster.Count-1) {
                            prevPixelBlack = false;
                            prevPixelBorder = true;
                            prevPixelHand = false;
                            depth_mid[3*(r * width + c)] = 255;
                            depth_mid[3*(r * width + c) + 1] = 255;
                            depth_mid[3*(r * width + c) + 2] = 255;
                            borderPixels[++borderCnt] = r * width + c;
                        }
                        else {
                            prevPixelBlack = false;
                            prevPixelBorder = false;
                            prevPixelHand = true;
                            depth_mid[3*(r * width + c)] = 50;
                            depth_mid[3*(r * width + c) + 1] = 100;
                            depth_mid[3*(r * width + c) + 2] = 50;
                        }
                        lastpoint = point;
                    }

                    prevPixelBlack = true;
                    prevPixelHand = false;
                    prevPixelBorder = false;
                    // Traverse the depth map column-wise to mark blank, border and hand pixels
                    for (int col = 0; col < width; col++) {
                        for (int row = 0; row < height; row++) {
                            int i = row * width + col;
                            if (depth_mid[3 * i + 0] != 0 && depth_mid[3 * i + 0] != 255) { // hand
                                if (prevPixelBlack) {
                                    depth_mid[3 * i + 0] = 255;
                                    depth_mid[3 * i + 1] = 255;
                                    depth_mid[3 * i + 2] = 255;
                                    borderPixels[++borderCnt] = i;
                                    prevPixelBlack = false;
                                    prevPixelBorder = true;
                                    prevPixelHand = false;
                                }
                                else {
                                    prevPixelHand = true;
                                    prevPixelBlack = false;
                                    prevPixelBorder = false;
                                    lastHandIndex = i;
                                }
                            }
                            else if (depth_mid[3 * i + 0] == 255) { // border
                                prevPixelBlack = false;
                                prevPixelBorder = true;
                                prevPixelHand = false;
                            }
                            else if (depth_mid[3 * i + 0] == 0) { // blank
                                if (prevPixelHand) {
                                    depth_mid[3 * lastHandIndex + 0] = 255;
                                    depth_mid[3 * lastHandIndex + 1] = 255;
                                    depth_mid[3 * lastHandIndex + 2] = 255;
                                    borderPixels[++borderCnt] = lastHandIndex;
                                }
                                prevPixelBlack = true;
                                prevPixelBorder = false;
                                prevPixelHand = false;
                            }
                        }
                    }

                    // Isolate true border pixels by removing false border pixels
                    for (int i = 0; i < bufferSize; i++) {
                        if (depth_mid[3 * i + 0] == 255) {
                            int r = i / width;
                            int c = i % width;
                            bool touchingHand = false, touchingBlack = false;
                            for (int cc = c - 1; cc <= c + 1; cc++) {
                                for (int rr = r - 1; rr <= r + 1; rr++) {
                                    int ii = rr * width + cc;
                                    if (ii >= 0 && ii < bufferSize &&
                                      depth_mid[3 * ii + 0] != 0 && depth_mid[3 * ii + 0] != 255) {
                                        // pixel <rr,cc> is adjacent to a hand pixel
                                        touchingHand = true;
                                    }
                                    if (ii >= 0 && ii < bufferSize && depth_mid[3 * ii + 0] == 0) {
                                        // pixel <rr,cc> is adjacent to a blank pixel
                                        touchingBlack = true;
                                    }
                                    if (touchingHand && touchingBlack) break;
                                }
                                if (touchingHand && touchingBlack) break;
                            }
                            if (!touchingBlack || !touchingHand) {
                                // Mark <rr,cc> as blank if it not adjacent to both blank and hand pixels
                                depth_mid[3 * i + 0] = 0;
                                depth_mid[3 * i + 1] = 0;
                                depth_mid[3 * i + 2] = 0;
                            }
                        }
                    }

                    List<int> contour = new List<int>();
                    // Compute the border contour
                    for (int i=0; i<bufferSize; i++) {
                        if (depth_mid[3*i+0] == 255 && depth_mid[3*i+1] == 255 && depth_mid[3*i+2] == 255) {
                            // Find contour containing pixel i
                            contour.Add(i);
                            segBuff[i] = 1;
                            for (int dir = 0; dir < 2; dir++) {
                                int curr = i;
                                while (true) {
                                    int r = curr / width;
                                    int c = curr % width;
                                    bool done = false;
    
                                    int ii = r * width + (c-1);
                                    if (ii < 0 || ii >= bufferSize || segBuff[ii] == 1 || depth_mid[3 * ii + 0] != 255) {
                                        ii = r * width + (c+1);
                                        if (ii < 0 || ii >= bufferSize || segBuff[ii] == 1 || depth_mid[3 * ii + 0] != 255) {
                                            ii = (r-1) * width + c;
                                            if (ii < 0 || ii >= bufferSize || segBuff[ii] == 1 || depth_mid[3 * ii + 0] != 255) {
                                                ii = (r+1) * width + c;
                                                if (ii < 0 || ii >= bufferSize || segBuff[ii] == 1 || depth_mid[3 * ii + 0] != 255) {
                                                    ii = (r-1) * width + (c-1);
                                                    if (ii < 0 || ii >= bufferSize || segBuff[ii] == 1 || depth_mid[3 * ii + 0] != 255) {
                                                        ii = (r - 1) * width + (c + 1);
                                                        if (ii < 0 || ii >= bufferSize || segBuff[ii] == 1 || depth_mid[3 * ii + 0] != 255) {
                                                            ii = (r + 1) * width + (c - 1);
                                                            if (ii < 0 || ii >= bufferSize || segBuff[ii] == 1 || depth_mid[3 * ii + 0] != 255) {
                                                                ii = (r + 1) * width + (c + 1);
                                                                if (ii < 0 || ii >= bufferSize || segBuff[ii] == 1 || depth_mid[3 * ii + 0] != 255) {
                                                                    done = true;
                                                                }
                                                            }
                                                        }
                                                    }
                                                }
                                            }
                                        }
                                    }
                                    if (done) break;
                                    if (dir == 0)
                                        contour.Add(ii);
                                    else
                                        contour.Insert(0, ii);
                                    segBuff[ii] = 1;
                                    curr = ii;
                                }
                            }
                            break;
                        }
                    }
                    //Console.WriteLine(borderCnt + " " + contour.Count);

                    int[] extContour = new int[50];
                    // Append first 50 contour points at the end of the contour
                    for (int counter = 0; counter < contour.Count && counter < 50;++counter) {
                        extContour[counter] = contour[counter];
                    }
                    for (int counter = 0; counter < 50; counter++) {
                        contour.Add(extContour[counter]);
                    }

                    // The value of K in the K-curvature algorithm
                    int curv = 30;
                    // Number of fingertip
                    int tipNum = 0;
                    // Detect fingertips as points on the contour, using K-curvature algorithm.
                    for (int z = 0; z < contour.Count; ++z) {
                        int i = contour[z];
                        int counter = z;
                        counter = Math.Min(counter + curv, contour.Count);
                        if (counter < contour.Count) {
                            int j = contour[counter];
                            int counter2 = counter;
                            counter2 = Math.Min(counter2 + curv, contour.Count);
                            if (counter2 < contour.Count) {
                                int k = contour[counter2];
                                float angle = getAngle(i, j, k, width, height);
                                if (angle < 30) {
                                    int center = getCentroid(i, j, k, width, height);
                                    if (depth_mid[3*center+0] != 255 && depth_mid[3*center+0] != 0) {
                                        // Fingertip is valid only if center pixel is a hand pixel.
                                        if (tipNum < 5) {
                                            int r = j / width + miny;
                                            int c = j % width + minx;
                                            fingers[tipNum] = r * depthBitmap.Width + c;
                                            tipNum++;
                                        }
                                        z = Math.Min(contour.Count, z + curv);
                                    }
                                }
                            }
                        }
                    }

                    //System.Drawing.Rectangle rect = new System.Drawing.Rectangle(0, 0, depthBitmap.Width, depthBitmap.Height);
                    //BitmapData data = this.depthBitmap.LockBits(rect, ImageLockMode.ReadWrite, System.Drawing.Imaging.PixelFormat.Format32bppArgb);
                    //byte[] test = new byte[data.Height * data.Stride];
                    //for (int i = 0; i < test.Length/4; i++) {
                    //    int r = i / depthBitmap.Width;
                    //    int c = i % depthBitmap.Width;
                    //    if (r < miny || r > maxy || c < minx || c > maxx) {
                    //        byte* a = (byte*)data.Scan0 + r * data.Stride;
                    //        test[i * 4] = a[c*4];
                    //        test[i * 4 + 1] = a[c * 4+1];
                    //        test[i * 4 + 2] = a[c * 4+2];
                    //        test[i * 4 + 3] = 255;
                    //    }
                    //    else {
                    //        r -= miny;
                    //        c -= minx;
                    //        test[i * 4] = (byte)depth_mid[3 * (r * width + c)];
                    //        test[i * 4 + 1] = (byte)depth_mid[3*(r * width + c) + 1];
                    //        test[i * 4 + 2] = (byte)depth_mid[3*(r * width + c) + 2];
                    //        test[i * 4 + 3] = 255;
                    //    }
                    //}
                    //depthImageTexture = new Texture2D(Game.GraphicsDevice, depthBitmap.Width, depthBitmap.Height, 1, TextureUsage.None, SurfaceFormat.Color);
                    //try {
                    //    depthImageTexture.SetData<byte>(test);
                    //}
                    //catch { }
                    //this.depthBitmap.UnlockBits(data);
                }
            }
        }

        private float getAngle(int i, int j, int k, int w, int h) {
            int x1 = i % w;
            int y1 = i / w;
            int x2 = j % w;
            int y2 = j / w;
            int x3 = k % w;
            int y3 = k / w;
            int d12 = (x2-x1)*(x2-x1) + (y2-y1)*(y2-y1);
            int d13 = (x3-x1)*(x3-x1) + (y3-y1)*(y3-y1);
            int d23 = (x2-x3)*(x2-x3) + (y2-y3)*(y2-y3);
            float theta = (float)Math.Acos((d13 - d12 - d23) / (-2 * Math.Sqrt(d12) * Math.Sqrt(d23)));
            return theta * 180f / (float)Math.PI;
        }
            
        private int getCentroid(int i, int j, int k, int w, int h) {
            int x1 = i % w;
            int y1 = i / w;
            int x2 = j % w;
            int y2 = j / w;
            int x3 = k % w;
            int y3 = k / w;
            int x = (x1+x2+x3)/3;
            int y = (y1+y2+y3)/3;
            return y * w + x;
        }*/
        #endregion

        /// <summary>
        /// Returns the depth camera's bitmap source as byte[] - not sure why i want this yet....
        /// </summary>
        /// 
        public void GenerateDepthImageSource()
        {

            UpdateHistogram(_depthMD);
            depthThreshholdBreakingPoints.Clear();
            unsafe
            {

                byte[] bytes;

                lock (this)
                {
                    System.Drawing.Rectangle rect = new System.Drawing.Rectangle(0, 0, this.depthBitmap.Width, this.depthBitmap.Height);
                    BitmapData data = this.depthBitmap.LockBits(rect, ImageLockMode.ReadWrite, System.Drawing.Imaging.PixelFormat.Format32bppArgb);
                    int bufferSize = data.Height * data.Stride;
                    bytes = new byte[bufferSize];
                    ushort* pDepth = (ushort*)this.DepthGenerator.GetDepthMapPtr().ToPointer();
                    RGB24Pixel* pixel = (RGB24Pixel*)ImageGenerator.GetImageMapPtr().ToPointer();

                    // set pixels
                    //pDepth += yMin * _depthMD.XRes + xMin;
                    for (int y = 0; y < _depthMD.YRes; ++y)//, pDepth += (_depthMD.XRes - xMax) + xMin)
                    {
                        byte* pDest = (byte*)data.Scan0 + y * data.Stride;
                        for (int x = 0; x < _depthMD.XRes; ++x, ++pDepth, ++pixel)
                        {
                            if ((lockedMode1 &&
                                Math.Abs(x - centroid1.X) < 100 &&
                                Math.Abs(y - centroid1.Y) < 100 &&
                                Math.Abs(*pDepth - centroid1.Z) < 100) || (
                                !lockedMode1 && *pDepth > zMin && *pDepth < zMax &&
                                x > xMin && x < xMax &&
                                y > yMin && y < yMax))
                            {
                                depthThreshholdBreakingPoints.Add(new Vector3(x, y, Convert.ToSingle(*pDepth)));
                                if (highlightCapturedPoints)
                                {
                                    pDest[x * 4] = 255;
                                    pDest[x * 4 + 1] = 0;
                                    pDest[x * 4 + 2] = 0;
                                    continue;
                                }
                            }
                            else if ((lockedMode2 &&
                                Math.Abs(x - centroid2.X) < 100 &&
                                Math.Abs(y - centroid2.Y) < 100 &&
                                Math.Abs(*pDepth - centroid2.Z) < 100) || (
                                !lockedMode2 && *pDepth > zMin && *pDepth < zMax &&
                                x > xMin && x < xMax &&
                                y > yMin && y < yMax))
                            {
                                depthThreshholdBreakingPoints.Add(new Vector3(x, y, Convert.ToSingle(*pDepth)));
                                if (highlightCapturedPoints)
                                {
                                    pDest[x * 4] = 0;
                                    pDest[x * 4 + 1] = 255;
                                    pDest[x * 4 + 2] = 0;
                                    continue;
                                }
                            }
                            else if (depthMode)
                            {
                                byte depthPixel = (byte)Histogram[*pDepth];
                                pDest[x * 4] = depthPixel;
                                pDest[x * 4 + 1] = depthPixel;
                                pDest[x * 4 + 2] = depthPixel;
                            }
                            else
                            {
                                pDest[x * 4] = (*pixel).nBlue;
                                pDest[x * 4 + 1] = (*pixel).nGreen;
                                pDest[x * 4 + 2] = (*pixel).nRed;
                            }
                            pDest[x * 4 + 3] = 255;
                        }
                    }
                    // copy bitmap data into buffer
                    Marshal.Copy(data.Scan0, bytes, 0, bytes.Length);
                    // copy our buffer to the texture

                    depthImageTexture = new Texture2D(Game.GraphicsDevice, depthBitmap.Width, depthBitmap.Height, 1, TextureUsage.None, SurfaceFormat.Color);
                    try
                    {
                        depthImageTexture.SetData<byte>(bytes);
                    }
                    catch { }

                    this.depthBitmap.UnlockBits(data);

                }

                Game._kinect.CalculateCentroids();

            }

        }

        public Vector3 GetCentroid1()
        {
            if (DateTime.Now.Ticks - lostTime1 < 10000000 || lockedMode1)
                return centroid1;
            else
                return new Vector3(xMin, yMid, zMax);
        }

        public Vector3 GetCentroid2()
        {
            if (DateTime.Now.Ticks - lostTime2 < 10000000 || lockedMode2)
                return centroid2;
            else
                return new Vector3(xMax, yMid, zMax);
        }

        public void CalculateCentroids()
        {
            cluster1.Clear();
            cluster2.Clear();
            clusterA.Clear();
            clusterB.Clear();
            minX1 = xMax;
            maxX1 = xMin;
            minX2 = xMax;
            maxX2 = xMin;
            minZ1 = zMax;
            minZ2 = zMax;
            if (lockedMode1)
                minZ1 = 100000000;
            if (lockedMode2)
                minZ2 = 100000000;


            lastcentroid1 = centroid1;
            lastcentroid2 = centroid2;

            Vector3 point;
            for (int i = 0; i < depthThreshholdBreakingPoints.Count; i++)
            {
                point = depthThreshholdBreakingPoints[i];
                if (getDistance(point, centroid1) <= getDistance(point, centroid2))
                {
                    if (point.Z < minZ1)
                        minZ1 = (int)point.Z;
                    cluster1.Add(point);
                }
                else
                {
                    if (point.Z < minZ2)
                        minZ2 = (int)point.Z;
                    cluster2.Add(point);
                }
            }

            foreach (Vector3 pointA in cluster1)
            {
                if (pointA.Z < (minZ1 + 60))
                {
                    clusterA.Add(pointA);
                    x = (int)pointA.X;
                    if (x > maxX1)
                        maxX1 = x;
                    if (x < minX1)
                        minX1 = x;
                }
            }

            foreach (Vector3 pointB in cluster2)
            {
                if (pointB.Z < (minZ2 + 60))
                {
                    clusterB.Add(pointB);
                    x = (int)pointB.X;
                    if (x > maxX2)
                        maxX2 = x;
                    if (x < minX2)
                        minX2 = x;
                }
            }

            if (clusterA.Count > 15000 || clusterA.Count < 50)
            {
                lockedMode1 = false;
                centroid1.X = xMin;
                centroid1.Y = yMid;
                centroid1.Z = zMax;
                for (int i = 0; i < 5; i++) {
                    fingers1[i] = 0;
                }
            }
            else
            {
                lostTime1 = DateTime.Now.Ticks;
                CalculateCentroid(clusterA, ref centroid1, lastcentroid1);
                delta1 = 0;// maxX1 - minX1;
                //threshold1 = 100;//105 - (centroid1.Z - 500.0) * .08;
                //c1Open = (delta1 > threshold1) || (centroid1.Y > 350);

                if (!lockedMode1 && centroid1.Z > 500 && !(centroid1.X == xMin && centroid1.Y == yMid && centroid1.Z == zMax))
                {
                    if (lastcentroid1.X == xMin && lastcentroid1.Y == yMid && lastcentroid1.Z == zMax)
                    {
                        lockTime1 = DateTime.Now.Ticks;
                    }
                    else
                    {
                        if (Vector3.Distance(centroid1, lastcentroid1) < 3 && DateTime.Now.Ticks - lockTime1 > 30000000)
                            lockedMode1 = true;
                    }
                }
                GenerateFingersCV(ref fingers1, clusterA, GetCentroid1(), minX1, maxX1);
                int count = 0;
                for (int i = 0; i < 5; i++)
                {
                    if (fingers1[i] != 0)
                    {
                        int fingerY = fingers1[i] / (xMax - xMin);
                        int fingerX = fingers1[i] % (xMax - xMin);
                        delta1 += getDistance(new Vector3(fingerX, fingerY, 0), centroid1);
                        count++;
                    }
                }
                if(count > 0)
                    delta1 /= count;
                //Console.WriteLine("Delta1 = " + delta1);
                threshold1 = 60;//105 - (centroid1.Z - 500.0) * .08;
                c1Open = (delta1 > threshold1) || (centroid1.Z > 1000) || (centroid1.Y > 350);
            }

            if (clusterB.Count > 15000 || clusterB.Count < 50)
            {
                lockedMode2 = false;
                centroid2.X = xMax;
                centroid2.Y = yMid;
                centroid2.Z = zMax;
                for (int i = 0; i < 5; i++) {
                    fingers2[i] = 0;
                }
            }
            else
            {
                lostTime2 = DateTime.Now.Ticks;
                CalculateCentroid(clusterB, ref centroid2, lastcentroid2);
                delta2 = 0;// maxX2 - minX2;
                //threshold2 = 100;//105 - (centroid2.Z - 500.0) * .08;
                //c2Open = (delta2 > threshold2) || (centroid2.Y > 350);

                if (!lockedMode2 && centroid2.Z > 500 && !(centroid2.X == xMin && centroid2.Y == yMid && centroid2.Z == zMax))
                {
                    if (lastcentroid2.X == xMin && lastcentroid2.Y == yMid && lastcentroid2.Z == zMax)
                    {
                        lockTime2 = DateTime.Now.Ticks;
                    }
                    else
                    {
                        if (Vector3.Distance(centroid2, lastcentroid2) < 3 && DateTime.Now.Ticks - lockTime2 > 30000000)
                            lockedMode2 = true;
                    }
                }
                GenerateFingersCV(ref fingers2, clusterB, GetCentroid2(), minX2, maxX2);
                int count = 0;
                for (int i = 0; i < 5; i++)
                {
                    if (fingers2[i] != 0)
                    {
                        int fingerY = fingers2[i] / (xMax - xMin);
                        int fingerX = fingers2[i] % (xMax - xMin);
                        delta2 += getDistance(new Vector3(fingerX, fingerY, 0), centroid2);
                        count++;
                    }
                }
                if (count > 0)
                    delta2 /= count;
                //Console.WriteLine("Delta1 = " + delta1);
                threshold2 = 60;//105 - (centroid1.Z - 500.0) * .08;
                c2Open = (delta2 > threshold2) || (centroid2.Z > 1000) || (centroid2.Y > 350);
            }
        }

        public double getDistance(Vector3 a, Vector3 b)
        {
            return Math.Sqrt(Math.Pow(b.X - a.X, 2) + Math.Pow(b.Y - a.Y, 2));
        }

        private void CalculateCentroid(List<Vector3> cluster, ref Vector3 centroid, Vector3 lastcentroid)
        {
            float xsum = 0;
            float ysum = 0;
            float zsum = 0;
            float centroidX;
            float centroidY;
            float centroidZ;
            Vector3 interimCentroid;

            if (cluster.Count > Game.clusterThreshold)
            {
                for (int i = 0; i < cluster.Count; i++)
                {
                    xsum += cluster[i].X;
                    ysum += cluster[i].Y;
                    zsum += cluster[i].Z;
                }

                centroidX = xsum / cluster.Count;
                centroidY = ysum / cluster.Count;
                centroidZ = zsum / cluster.Count;

                interimCentroid = new Vector3(centroidX, centroidY, centroidZ);
                if ((getDistance(lastcentroid, interimCentroid) > Game.motionThreshold )) // smoothing
                    centroid = interimCentroid;
                
            }



        }

        #endregion

        #region OpenNI properties

        /// <summary>
        /// OpenNI main Context.
        /// </summary>
        public Context Context { get; private set; }

        /// <summary>
        /// OpenNI image generator.
        /// </summary>
        public ImageGenerator ImageGenerator { get; private set; }

        /// <summary>
        /// OpenNI depth generator.
        /// </summary>
        public DepthGenerator DepthGenerator { get; private set; }

        /// <summary>
        /// OpenNI histogram.
        /// </summary>
        public int[] Histogram { get; private set; }

        #endregion

        #endregion

        #region Constructor

        /// <summary>
        /// Creates a new instance of NuiSensor with the default configuration file.
        /// </summary>
        public NuiSensor(Main g, float xMin, float xMax, float yMin, float yMax, float zMin, float zMax)
            : this(CONFIGURATION, g)
        {
            this.xMin = (int)xMin;
            this.xMax = (int)xMax;
            this.yMin = (int)yMin;
            this.yMax = (int)yMax;
            this.zMin = (int)zMin;
            this.zMax = (int)zMax;
            xMid = (this.xMax - this.xMin) / 2;
            yMid = (this.yMax - this.yMin) / 2;
        }
        private Main Game;

        /// <summary>
        /// Creates a new instance of NuiSensor with the specified configuration file.
        /// </summary>
        /// <param name="configuration">Configuration file path.</param>
        public NuiSensor(string configuration, Main g)
        {
            Game = g;
            InitializeCentroids();
            InitializeCamera(configuration);
            depthImageTexture = new Texture2D(Game.GraphicsDevice, 640, 480, 1, TextureUsage.None, SurfaceFormat.Color);
            depthBitmap = new Bitmap(640, 480);
            InitializeThread();
            previousDepthImage = new ushort[640 * 480];
            //lockedMode = false;
        }

        private void InitializeCentroids()
        {
            lastcentroid1 = new Vector3(xMin, yMid, zMax);
            lastcentroid2 = new Vector3(xMax, yMid, zMax);
            centroid1 = new Vector3(xMin, yMid, zMax);
            centroid2 = new Vector3(xMax, yMid, zMax);
            cluster1 = new List<Vector3>();
            cluster2 = new List<Vector3>();
        }

        #endregion

        #region Private methods

        /// <summary>
        /// Initializes the image and depth camera.
        /// </summary>
        /// <param name="configuration">Configuration file path.</param>
        private void InitializeCamera(string configuration)
        {
            try
            {
                Context = new Context(configuration);
            }
            catch
            {
                throw new Exception("Configuration file not found.");
            }

            ImageGenerator = Context.FindExistingNode(NodeType.Image) as ImageGenerator;
            DepthGenerator = Context.FindExistingNode(NodeType.Depth) as DepthGenerator;
            MapOutputMode mapOutputMode = new MapOutputMode();
            mapOutputMode.nFPS = 30;
            mapOutputMode.nXRes = 640;
            mapOutputMode.nYRes = 480;

            DepthGenerator.SetMapOutputMode(mapOutputMode);
            DepthGenerator.GetAlternativeViewPointCap().SetViewPoint(ImageGenerator);
            Histogram = new int[DepthGenerator.GetDeviceMaxDepth()];
        }


        /// <summary>
        /// Initializes the background camera thread.
        /// </summary>
        private void InitializeThread()
        {
            _isRunning = true;

            _cameraThread = new Thread(CameraThread);
            _cameraThread.IsBackground = true;
            _cameraThread.Start();
        }

        /// <summary>
        /// Updates image and depth values.
        /// </summary>
        private unsafe void CameraThread()
        {
            while (_isRunning)
            {
                Context.WaitAndUpdateAll();

                ImageGenerator.GetMetaData(_imgMD);
                DepthGenerator.GetMetaData(_depthMD);
                int nMiddleIndex = 640 * 240 + 320;
                GenerateDepthImageSource();
            }
        }

        #endregion

        #region Public methods

        /// <summary>
        /// Re-creates the depth histogram.
        /// </summary>
        /// <param name="depthMD"></param>
        public unsafe void UpdateHistogram(DepthMetaData depthMD)
        {
            // Reset.
            for (int i = 0; i < Histogram.Length; ++i)
                Histogram[i] = 0;

            ushort* pDepth = (ushort*)depthMD.DepthMapPtr.ToPointer();

            int points = 0;
            for (int y = 0; y < depthMD.YRes; ++y)
            {
                for (int x = 0; x < depthMD.XRes; ++x, ++pDepth)
                {
                    ushort depthVal = *pDepth;
                    if (depthVal != 0)
                    {
                        Histogram[depthVal]++;
                        points++;
                    }
                }
            }

            for (int i = 1; i < Histogram.Length; i++)
            {
                Histogram[i] += Histogram[i - 1];
            }

            if (points > 0)
            {
                for (int i = 1; i < Histogram.Length; i++)
                {
                    Histogram[i] = (int)(256 * (1.0f - (Histogram[i] / (float)points)));
                }
            }
        }

        /// <summary>
        /// Releases any resources.
        /// </summary>
        public void Dispose()
        {

            _isRunning = false;
            _cameraThread.Join();
            Context.Dispose();
            _cameraThread = null;
            Context = null;
        }

        #endregion

        #region INUI Members

        Texture2D INUI.GetDepthImage()
        {

            // int numberOfPixelsCloserThan1000ml = 0;
            // for (int x = 1000; x > 0; x--)
            // {
            //     numberOfPixelsCloserThan1000ml += Histogram[x];
            // }
            //Console.Out.WriteLine(numberOfPixelsCloserThan1000ml);
            return depthImageTexture;
        }
        /*
        Texture2D INUI.GetRGBImage()
        {
            MapData<RGB24Pixel> map = ImageGenerator.GetRGB24ImageMap();
            Texture2D texture = new Texture2D(Game.GraphicsDevice, 640, 480, 1, TextureUsage.None, SurfaceFormat.Color);
            RGB24Pixel[] array = new RGB24Pixel[map.XRes*map.YRes];
            for(int i = 0; i < array.Length; i++)
                array[i] = map[i];
            texture.SetData<RGB24Pixel>(array);

            return texture;
        }*/

        public VertexPositionColor[] GetDepthImagePointCloud()
        {
            throw new NotImplementedException();
        }

        #endregion
    }
}

#region Convex Hull Helper Classes
class Polysort {
    private static void swap<T>(T[] arr, int s, int t) {
        T tmp = arr[s]; arr[s] = arr[t]; arr[t] = tmp;
    }

    // Typed OO-style quicksort a la Hoare/Wirth

    private static void qsort<T>(Ordered<T>[] arr, int a, int b) {
        // sort arr[a..b]
        if (a < b) {
            int i = a, j = b;
            Ordered<T> x = arr[(i + j) / 2];
            do {
                while (arr[i].Less(x)) i++;
                while (x.Less(arr[j])) j--;
                if (i <= j) {
                    swap<Ordered<T>>(arr, i, j);
                    i++; j--;
                }
            } while (i <= j);
            qsort<T>(arr, a, j);
            qsort<T>(arr, i, b);
        }
    }

    public static void Quicksort<T>(Ordered<T>[] arr) {
        qsort<T>(arr, 0, arr.Length - 1);
    }
}

public abstract class Ordered<T> {
    public abstract bool Less(Ordered<T> that);
}

class CDLL<T> {
    private CDLL<T> prev, next;     // not null, except in deleted elements
    public T val;

    // A new CDLL node is a one-element circular list
    public CDLL(T val) {
        this.val = val; next = prev = this;
    }

    public CDLL<T> Prev {
        get { return prev; }
    }

    public CDLL<T> Next {
        get { return next; }
    }

    // Delete: adjust the remaining elements, make this one point nowhere
    public void Delete() {
        next.prev = prev; prev.next = next;
        next = prev = null;
    }

    public CDLL<T> Prepend(CDLL<T> elt) {
        elt.next = this; elt.prev = prev; prev.next = elt; prev = elt;
        return elt;
    }

    public CDLL<T> Append(CDLL<T> elt) {
        elt.prev = this; elt.next = next; next.prev = elt; next = elt;
        return elt;
    }

    public int Size() {
        int count = 0;
        CDLL<T> node = this;
        do {
            count++;
            node = node.next;
        } while (node != this);
        return count;
    }

    public void PrintFwd() {
        CDLL<T> node = this;
        do {
            Console.WriteLine(node.val);
            node = node.next;
        } while (node != this);
        Console.WriteLine();
    }

    public void CopyInto(T[] vals, int i) {
        CDLL<T> node = this;
        do {
            vals[i++] = node.val;	// still, implicit checkcasts at runtime 
            node = node.next;
        } while (node != this);
    }
}

class PointCV : Ordered<PointCV> {
    private static readonly Random rnd = new Random();

    public double x, y;

    public PointCV(double x, double y) {
        this.x = x; this.y = y;
    }

    public override string ToString() {
        return "(" + x + ", " + y + ")";
    }

    public static PointCV Random(int w, int h) {
        return new PointCV(rnd.Next(w), rnd.Next(h));
    }

    public bool Equals(PointCV p2) {
        return x == p2.x && y == p2.y;
    }

    public override bool Less(Ordered<PointCV> o2) {
        PointCV p2 = (PointCV)o2;
        return x < p2.x || x == p2.x && y < p2.y;
    }

    // Twice the signed area of the triangle (p0, p1, p2)
    public static double Area2(PointCV p0, PointCV p1, PointCV p2) {
        return p0.x * (p1.y - p2.y) + p1.x * (p2.y - p0.y) + p2.x * (p0.y - p1.y);
    }
}

class Convexhull {
    public static PointCV[] convexhull(PointCV[] pts) {
        // Sort points lexicographically by increasing (x, y)
        int N = pts.Length;
        Polysort.Quicksort<PointCV>(pts);
        PointCV left = pts[0], right = pts[N - 1];
        // Partition into lower hull and upper hull
        CDLL<PointCV> lower = new CDLL<PointCV>(left), upper = new CDLL<PointCV>(left);
        for (int i = 0; i < N; i++) {
            double det = PointCV.Area2(left, right, pts[i]);
            if (det > 0)
                upper = upper.Append(new CDLL<PointCV>(pts[i]));
            else if (det < 0)
                lower = lower.Prepend(new CDLL<PointCV>(pts[i]));
        }
        lower = lower.Prepend(new CDLL<PointCV>(right));
        upper = upper.Append(new CDLL<PointCV>(right)).Next;
        // Eliminate points not on the hull
        eliminate(lower);
        eliminate(upper);
        // Eliminate duplicate endpoints
        if (lower.Prev.val.Equals(upper.val))
            lower.Prev.Delete();
        if (upper.Prev.val.Equals(lower.val))
            upper.Prev.Delete();
        // Join the lower and upper hull
        PointCV[] res = new PointCV[lower.Size() + upper.Size()];
        lower.CopyInto(res, 0);
        upper.CopyInto(res, lower.Size());
        return res;
    }

    // Graham's scan
    private static void eliminate(CDLL<PointCV> start) {
        CDLL<PointCV> v = start, w = start.Prev;
        bool fwd = false;
        while (v.Next != start || !fwd) {
            if (v.Next == w)
                fwd = true;
            if (PointCV.Area2(v.val, v.Next.val, v.Next.Next.val) < 0) // right turn
                v = v.Next;
            else {                                       // left turn or straight
                v.Next.Delete();
                v = v.Prev;
            }
        }
    }
}
#endregion